//
// Created by rebeater on 5/7/21.
//

#include "nav_io.h"
#include <iostream>
#include <iomanip>
using namespace std;
ostream & operator<<(ostream & os,GnssData &gnss){
    os<<fixed<<setprecision(0) <<gnss.week<<" " <<setprecision(5) <<gnss.gpst<<" "
      <<setprecision(12)<<gnss.lat<<" "<<gnss.lon<<" "<<setprecision(3)<<gnss.height<<" "
      <<setprecision(3)<<gnss.pos_std[0]<<" "<<gnss.pos_std[1]<<" "<<gnss.pos_std[2]<<"  " <<gnss.hdop<<" "
      <<fixed<<gnss.ns<<" "<< gnss.mode;
    return os;
}

ostream & operator<<(ostream & os,ImuData &imu){
    os<<fixed<<setprecision(6)<<imu.gpst<<" ";
    os<<setprecision(7)<<imu.gyro[0]<<" "<<imu.gyro[1]<<" "<<imu.gyro[2]<<" ";
    os<<setprecision(7)<<imu.acce[0]<<" "<<imu.acce[1]<<" "<<imu.acce[2]<<" ";
    return os;
}
ostream & operator<<(ostream & os,CameraEvent &cam){
  os<<fixed<<setprecision(6)<<cam.time_s<<" ";
	os<<cam.camera_info_.ImageName<<" "
	<<cam.camera_info_.Vn[0] << " " <<cam.camera_info_.Vn[1] << " "<<cam.camera_info_.Vn[2] << " "
	<<cam.camera_info_.VnStd[1] << " " <<cam.camera_info_.VnStd[1] << " "<<cam.camera_info_.VnStd[2] << " ";
  return os;
}

ostream & operator<<(ostream & os,Velocity &vel){
    os<<fixed<<setprecision(6)<<vel.time_s<<" ";
    os<<setprecision(7)<<vel.forward<<" "<<vel.angular;
    return os;
}
ostream &operator<<(ostream &os, NavOutput output) {
    os << 0 << "  " <<fixed<< setprecision(6) << output.gpst << "   ";
    os << left << setprecision(15) << output.pos[0] / _deg << "   " << output.pos[1] / _deg << "   " << output.pos[2]
       << "   ";
    os << left << setprecision(6) << output.vn[0] << "   " << output.vn[1] << "   " << output.vn[2] << "   ";
    os << left << setprecision(6) << output.atti[0] / _deg << "   " << output.atti[1] / _deg << "   "
       << output.atti[2] / _deg << "   ";
    /* os << left << setprecision(10) << output.gb[0] / _deg * _hour << "   " << output.gb[1] / _deg * _hour << "   "
        << output.gb[2] / _deg * _hour << "  ";
     os << left << setprecision(10) << output.ab[0] / _deg * _hour << "   " << output.ab[1] / _deg * _hour << "   "
        << output.ab[2] / _deg * _hour;*/
    return os;
}
